//-----------------------------------------------------------------------
//  This file is part of the Microsoft Robotics Studio Code Samples.
//
//  Copyright (C) Microsoft Corporation.  All rights reserved.
//  Copyright (C) Parallax, Inc. All Rights Reserved.
//
//  Updated Nov-2007 by Trevor Taylor, Software Technology, Australia
//
//  Change Log:
//  Exposed the AutonomousMode flag via the config file.
//
//  Flash LEDs and sound a tone to let user know that the robot is alive.
//
//  Added queuing of motion requests.
//
//  Added new methods to handle Set Pins, Play Tone and Execute Behavior.
//
//  Jan-2008:
//  Added a version into the protocol to easily keep track of what is
//  in the Boe-Bot
//
//  $File: BoeBotControl.cs $ $Revision: 10 $
//-----------------------------------------------------------------------

using System;
using System.Collections.Generic;
using System.Text;
using System.IO.Ports;
using System.Threading;
using Microsoft.Robotics.Services.BasicStamp2;
// TT
using System.Collections;


namespace Microsoft.Robotics.Services.BoeBot
{
    /// <summary>
    /// All concurrency is done by calling service. Assume this is not thread safe
    /// </summary>
    internal class BoeBotControl
    {
        SerialPort _serialPort;
        BasicStamp2Operations _bs2Port = null;

        // TT - Change this to true to allow the Boe-Bot to wander around
        // Also enable the Whiskers in ExecuteMain()
        // Was called _autonMode in V1.0 (to match the public name)
        bool _autonomousMode = false;
        public bool AutonMode
        {
            get { return _autonomousMode; }
            set 
            { 
                _autonomousMode = value; 
                pwmLeft = 100; 
                pwmRight = 100; 
            }
        }

        bool connected = false;
        bool running = false;

        public bool Running
        {
            get { return running; }
            set { running = value; }
        }
        // Modified for CF version
        //int _delay = 0;
        int _delay = 100;
        public int Delay
        {
            get { return _delay; }
            set { _delay = value; }
        }

        // Version number obtained from the Boe-Bot's synch requests
        public int MajorVersion = 0;
        public int MinorVersion = 0;
        //private bool versionSet = false;

        internal int frameCounter = 0;
        int pwmLeft = 100;
        int pwmRight = 100;
        byte msgCnt = 0;          

        bool priorIrLeft = false;
        bool irLeft = false;
        bool priorIrRight = false;
        bool irRight = false;

        bool digFlag = false;               // Digital sensor values returned with each command
        bool irFlag = false;                // IR info processed
        bool wFlag = false;                 // Whisker (contact sensor) info processed

        bool wLeft = false;                 // Whisker states
        bool wRight = false;
        bool priorWhiskerLeft = false;
        bool priorWhiskerRight = false;

        bool handshake = false;             // Handshake after BT connection

        AutonomousDirection autonDir = AutonomousDirection.FWD;
        const int turnCounts = 5;
        //int ctr = 0;

        // TT
        private Queue cmdQueue = new Queue();
        //private Queue syncQueue;


        #region Constructors
        public BoeBotControl(BasicStamp2Operations bs2Port)
        {
            _bs2Port = bs2Port;
            // Synchronized queues are not supported on CF
            //syncQueue = Queue.Synchronized(cmdQueue);
        }

        ~BoeBotControl()
        {
            Close();
        }
        #endregion

        public bool Connect(int serialPort, out string errorMessage)
        {
            if (serialPort <= 0)
            {
                errorMessage = "The Boe-Bot serial port is not configured!";
                return false;
            }

            if (connected)
            {
                Close();
            }

            try
            {
                string ComPort = "COM" + serialPort.ToString();
                _serialPort = new SerialPort(ComPort, 9600, Parity.None, 8, StopBits.One);

                // Added for CF version
                _serialPort.WriteTimeout = 250;
                _serialPort.ReadTimeout = 250;

                _serialPort.Open();
                _serialPort.DiscardInBuffer();
                errorMessage = string.Empty;
                connected = true;
                return true;
            }
            catch (Exception ex)
            {
                errorMessage = string.Format("Error connecting Boe-Bot to COM{0}: {1}", serialPort, ex.Message);
                return false;
            }
        }

        public void Close()
        {
            if (!connected)
                return;
            connected = false;

            if (running)
            {
                running = false;
                Thread.Sleep(100);
            }

            if (_serialPort != null && _serialPort.IsOpen)
            {
                _serialPort.Close();
                _serialPort = null;
            }

        }

        public void Start()
        {
            running = true;
        }

        public void Stop()
        {
            running = false;
        }

        /// <summary>
        /// -1.0 to 1.0
        /// </summary>
        /// <param name="left"></param>
        /// <param name="right"></param>
        public void SetSpeed(double? left, double? right)
        {
            if ((left != null && (left < -1.0 || left > 1.0)) 
                || ((right != null) && (right < -1.0 || right > 1.0))) 
                throw new SystemException("Invalid Speed!");

            if (left != null)
                pwmLeft = (int)((left * 100) + 100);

            if (right != null)
                pwmRight = (int)((-right * 100) + 100);
        }

        public void GetIR(ref bool left, ref bool right)
        {
            left = irLeft;
            right = irRight;
        }

        public void PollIR(ref bool left, ref bool right)
        {
            byte[] buf = new byte[5];
            buf[2] = 64; 
            buf[3] = buf[4] = 0;
            SendPackets(ref buf);

            left = (buf[3] & 2) > 0;
            right = (buf[3] & 1) > 0;
            //
            // Reverse the ir flags...
            //
            left = !left;
            right = !right;
        }

        public void GetWhiskers(ref bool left, ref bool right)
        {
            left = wLeft;
            right = wRight;
        }
      
        public void PollWhiskers(ref bool left, ref bool right)
        {
            byte[] buf = new byte[5];
            buf[2] = 65;
            buf[3] = buf[4] = 0;
            SendPackets(ref buf);

            left = (buf[3] & 8) > 0;
            right = (buf[3] & 4) > 0;
            //
            // Reverse the ir flags...
            //
            left = !left;
            right = !right;
        }

        internal void ExecuteMain()
        {
            if (!this.running)
            {
                Thread.Sleep(250);
                return;
            }
            if (!_autonomousMode)
            {
                // TT Oct-2007 - Insert additional commands
                lock (cmdQueue.SyncRoot)
                {
                    //if (syncQueue.Count > 0)
                    if (cmdQueue.Count > 0)
                    {
                        //byte[] cmd = (byte[])syncQueue.Dequeue();
                        byte[] cmd = (byte[])cmdQueue.Dequeue();
                        SendPackets(ref cmd);
                    }
                    else
                        SendSpeed(pwmLeft, pwmRight);
                }
            }

            // <--- 
            if (!handshake)
            {
                handshake = WaitForConnect();
                irFlag = EnableIr();
                // TT - Enable the Whiskers only in Autonomous Mode
                // Should normally be disabled and the code here will handle them.
                // When they are enabled, the Boe-Bot will stop if a whisker is pressed.
                // The code here has to execute a behavior or the Boe-Bot will stop forever.
                if (_autonomousMode)
                    wFlag = EnableWhiskers();
                else
                    wFlag = DisableWhiskers();
                digFlag = EnableDigitalSensors();
                for(int i = 0; i < 2; i++)
                {
                    // TT - Flashes both LEDs and sounds a tone
                    SetPins(3, 14, 3, 15);
                    SpeakerTone(50, 4000);
                    SetPins(4, 14, 4, 15);
                }
            }
            else
            {
                frameCounter++;

                if (!digFlag)
                {
                    PollIR(ref irLeft, ref irRight);
                    PollWhiskers(ref wLeft, ref wRight);
                }

                if (irLeft != priorIrLeft 
                    || irRight != priorIrRight
                    || wLeft != priorWhiskerLeft 
                    || wRight != priorWhiskerRight)
                {
                    priorIrLeft = irLeft;
                    priorIrRight = irRight;
                    priorWhiskerLeft = wLeft;
                    priorWhiskerRight = wRight;
                    PostIRNotification(irLeft, irRight, wLeft, wRight);
                }

                if (_autonomousMode)
                {
                    if (wFlag)
                    {
                        if (wLeft && wRight)
                        {
                            Maneuver('U');
                        }
                        else if (!wLeft && wRight)
                        {
                            Maneuver('L');
                        }
                        else if (wLeft && !wRight)
                        {
                            Maneuver('R');
                        }
                    }
                    if (irFlag)
                    {
                        if (irLeft && irRight)
                        {
                            autonDir = AutonomousDirection.BKWD;
                        }
                        else if (!irLeft && irRight)
                        {
                            autonDir = AutonomousDirection.LEFT;
                        }
                        else if (irLeft && !irRight)
                        {
                            autonDir = AutonomousDirection.RIGHT;
                        }
                        else
                        {
                            autonDir = AutonomousDirection.FWD;
                        }

                        switch (autonDir)
                        {
                            case AutonomousDirection.FWD:
                                {
                                    pwmLeft = 200;
                                    pwmRight = 0;
                                } break;

                            case AutonomousDirection.LEFT:
                                {
                                    pwmLeft = 0;
                                    pwmRight = 0;
                                } break;

                            case AutonomousDirection.RIGHT:
                                {
                                    pwmLeft = 200;
                                    pwmRight = 200;
                                } break;

                            case AutonomousDirection.BKWD:
                                {
                                    pwmLeft = 0;
                                    pwmRight = 200;
                                } break;
                        }
                        SendSpeed(pwmLeft, pwmRight);
                    }
                }
            }
            if (Delay > 0)
            {
                // TT - Don't do this in live code - only for debugging
                //Console.WriteLine("Sleeping...");
                Thread.Sleep(Delay);
            }
        }

        private void PostIRNotification(bool irLeft, bool irRight, bool whiskerLeft, bool whiskerRight)
        {
            SensorsChanged updateRequest = new SensorsChanged();
            updateRequest.Body = new Sensors();
            updateRequest.Body.IRLeft = irLeft;
            updateRequest.Body.IRRight = irRight;
            updateRequest.Body.WhiskerLeft = whiskerLeft;
            updateRequest.Body.WhiskerRight = whiskerRight;
            _bs2Port.Post(updateRequest);
        }


        #region Control

        // Sets speaker tone for ms duration in Hz.  Units of 50 are transmitted to fit them in bytes.
        private void SpeakerTone(int ms, int Hz)
        {
            byte[] buf = new byte[5];
            buf[2] = 96;
            buf[3] = (byte) (ms / 50);
            buf[4] = (byte) (Hz / 50);
            SendPackets(ref buf);
        }

        /**  
        *   Sets up to two pins.  buf[3] high nibble specifies the operation, and the
        *   low nibble specifies the pin.  The same applies to buf[4].
        *
        *   High nibble:
        *      0 - no action
        *      1 - set to output
        *      2 - set to input
        *      3 - set to (output-high)
        *      4 - set to (output-low)
        *   Low Nibble:
        *      0 to 15 - Specifies I/O pin.
        *      DO NOT try to set 0, 1, 5, or 6 because these are the EB500.
        */
        private void SetPins(byte op1, byte pin1, byte op2, byte pin2)
        {
            byte[] buf = new byte[5];
            buf[2] = 97;
            buf[3] = (byte) ((op1 * 16) + pin1);
            buf[4] = (byte) ((op2 * 16) + pin2);
            SendPackets(ref buf);
        }

        private void BotDelay(int time)
        // Boe-Bot does nothing for a time in ms.
        {
            byte[] buf = new byte[5];
            buf[2] = 98;
            buf[3] = (byte)(time % 256);
            buf[4] = (byte)(time / 256);
            SendPackets(ref buf);
        }
        #endregion

        #region Configuration

        // Causes digital sensors to be sent in buf[3] of every reply from the Boe-Bot.
        private bool EnableDigitalSensors()
        {
            byte[] buf = new byte[5];
            buf[2] = 128;
            SendPackets(ref buf);
            digFlag = true;
            return true;
        }

        // Boe-Bot stops and waits for reply after whisker contact.  If Boe-Bot kept moving,
        // delay in reply would cause collision.
        private bool EnableWhiskers()
        {
            byte[] buf = new byte[5];
            buf[2] = 130;
            SendPackets(ref buf);
            return true;
        }
        
        // Boe-Bot takes no action and does not report whisker contact.
        private bool DisableWhiskers()
        {
            byte[] buf = new byte[5];
            buf[2] = 162;
            SendPackets(ref buf);
            return false;
        }
        
        // Not implemented
        private bool EnableIr()
        {
            byte[] buf = new byte[5];
            buf[2] = 129;
            SendPackets(ref buf);
            return true;
        }

        // Not implemented.
        private bool DisableIr()
        {
            byte[] buf = new byte[5];
            buf[2] = 161;
            SendPackets(ref buf);
            return false;
        }
        #endregion

        #region Navigation

        // Send the wheel speeds to the Boe-Bot
        // 0 is full speed clockwise.  200 is full speed counterclockwise.  100 is stopped.
        private void SendSpeed(int left, int right)
        {
            byte[] buf = new byte[5];
            buf[2] = 32;
            buf[3] = (byte)left;
            buf[4] = (byte)right;
            SendPackets(ref buf);
        }

        private void Maneuver(char m)
        {
            byte[] buf = new byte[5];
            buf[2] = 33;
            buf[3] = (byte)m;
            SendPackets(ref buf);
        }

        // TT Oct-2007 - Add a new method to handle behaviors
        // This is an extension of the Maneuver method
        public void BehaviorRequest(char m, int duration)
        {
            byte[] buf = new byte[5];
            buf[2] = 33;
            buf[3] = (byte)m;
            buf[4] = (byte)duration;
            //syncQueue.Enqueue(buf);
            lock (cmdQueue.SyncRoot)
            {
                cmdQueue.Enqueue(buf);
            }
        }

        // TT Oct-2007 - Add a request to set the pins
        // NOTE: This does NO error checking and it cannot know which pins are which.
        // If you set the wrong pins, then you will upset your Boe-Bot!
        // Pin operations are in an enum called PinOperations
        public void SetPinsRequest(byte op1, byte pin1, byte op2, byte pin2)
        {
            byte[] buf = new byte[5];
            buf[2] = 97;
            buf[3] = (byte)((op1 * 16) + pin1);
            buf[4] = (byte)((op2 * 16) + pin2);
            //syncQueue.Enqueue(buf);
            lock (cmdQueue.SyncRoot)
            {
                cmdQueue.Enqueue(buf);
            }
        }

        // TT Oct-2007 - Add a request to play a tone (sound)
        // Sets speaker tone for ms duration in Hz.
        // Units of 50 are transmitted to fit them into bytes.
        public void PlayToneRequest(int Hz, int ms)
        {
            byte[] buf = new byte[5];
            buf[2] = 96;
            buf[3] = (byte)(ms / 50);
            buf[4] = (byte)(Hz / 50);
            //syncQueue.Enqueue(buf);
            lock (cmdQueue.SyncRoot)
            {
                cmdQueue.Enqueue(buf);
            }
        }

        #endregion

        #region Communication
        /**
         *  Handshake happens here.  192 causes the Boe-Bot's program to restart.  It sends a 1, 
         *  PC replies with 2, then Boe-Bot replies with 3.  If Boe-Bot want's to restart handshake,
         *  it can send a 1.
         */
        private bool WaitForConnect()
        {
            // TT Oct-2007 - Added to see resets
            Console.WriteLine("Connecting ...");

            // Send a reset request
            byte[] buf = new byte[5];
            buf[1] = 0;
            buf[2] = 192;
            SendPackets(ref buf);

            // Send handshake confirmations until acknowledged
            do
            {
                if (!running)
                {
                    break;
                }

                buf[2] = (byte)2;
                SendPackets(ref buf);
            } while (buf[2] != (byte)3);

            msgCnt = (byte)buf[1];

            // Get the version number
            // NOTE: This will be 0.0 for anything prior to the addition of the
            // version information in the protocol
            MajorVersion = buf[3];
            MinorVersion = buf[4];

            // TT Jan-2008 - Display version number
            Console.WriteLine("Boe-Bot Version " + MajorVersion + "." + MinorVersion);

            // TT Oct-2007 - Added to see resets
            Console.WriteLine("... Connected");

            return true;
        }

        /**
         *  Now, packet is sent repeatedly until reply is received from the Boe-Bot.
         */
        private bool SendPackets(ref byte[] packet)
        {
            // TT Oct-2007 - Add a counter for timeout on sending
            int counter = 0;

            if (packet.Length != 5) throw new SystemException("Invalid packet length!");
            packet[0] = 255;                                            // Start byte.
            packet[1] = msgCnt;                                         // Message index, incremened by Boe-Bot.
                                                                        // Must be used in next message that is
                                                                        // sent.

            // TT - NOTE: This code is not robust. If the Boe-Bot is turned off,
            // or the connection is lost, then there will be an exception on the
            // Write or Read. Also, it can become an infinite loop and the robot
            // becomes unresponsive.
            while (_serialPort.BytesToRead < 5)
            {
                _serialPort.Write(packet, 0, 5);
                while (_serialPort.BytesToWrite > 0) Thread.Sleep(1);
                counter++;
                if (counter > 10000)
                {
                    handshake = false;
                    return false;
                }
            }
            _serialPort.Read(packet, 0, 5);                             // Get Boe-Bot's reply.
            msgCnt = packet[1];                                         // Get next message count.
            if (digFlag)                                                // If EnableDigitalSensors = true,
            {                                                           // Boe-Bot replies with digital sensor
                //                                                      // values.
                // Use active-high convention
                //
                irLeft = !((packet[3] & 2) > 0);
                irRight = !((packet[3] & 1) > 0);
                wLeft = !((packet[3] & 8) > 0);
                wRight = !((packet[3] & 4) > 0);
            }

            if (packet[2] == 1)
            {
                // Boe-Bot requests reconnect
                handshake = false;
            }
            return true;
        }
        #endregion
    }
    enum AutonomousDirection { FWD, LEFT, RIGHT, BKWD }
    enum CommandLoop { STOP = 0, START }
}
